﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Systems;
using Drifting.Graphics.EffectObjects;
using Drifting.Graphics.PhysicalObjects.Vehicles;
using Drifting.Foundation;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;

using Drifting.MotionPlanning.Primitives;
using Drifting.Graphics.Visualizers;
using Drifting.MotionPlanning.SingleThreading.Planners;
using Drifting.MotionPlanning.SingleThreading.Algorithms;

namespace Drifting.Scenes
{
    public class Comparison_RRTstar : Basic
    {
        Planner_Demo planner;

        public Comparison_RRTstar(ref Matrix view, ref Matrix projection, GraphicsDevice device, Camera camera, Game1 game)
            : base(ref view, ref projection, device, camera, game)
        {
            camera.Camera_InitView(0.001f, 10, 0, 0, 0, 0, (float)(Math.PI / 3.2f), ref view, ref projection);
            grid.XYZ = false;

            BaseSystem point2D = new Point2D();
            Skin_Point2DGraph point_visual = new Skin_Point2DGraph(new List<State>(), device, point2D.SimpleEnv);
            
            point_visual.Preview = Skin_G.PreviewResolution.Full;
            double[] q_zero = new double[] { 0, 0 };

            TreeBasedPlanningAlgorithm algorithm = new RRTstar(point2D, new Random());
            planner = new Planner_Demo(algorithm, device, point_visual, inXgoal);
            point_visual.SetPlanner(algorithm);
            algorithm.SetRoot(q_zero);

            planner.Viewable = true;
            planner.Start(1000000);



        }

        override public bool inXgoal(double[] q)
        {
            if (q[0] < 4.25)
                return false;
            if (q[0] > 4.75)
                return false;

            if (q[1] < 4.25)
                return false;
            if (q[1] > 4.75)
                return false;

            return true;
        }

        override public Color BackgroundColor()
        {
            return Color.White;
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            base.Draw(camera, projection);
            planner.Skin.Draw(camera, projection);
        }
    }
}
